#include <rclcpp/rclcpp.hpp>
#include <serial_driver/serial_driver.hpp>
#include <iostream>
#include <memory>
#include <chrono>

using namespace std::chrono_literals;

const unsigned char ender[2] = {0x0D, 0x0A};
const unsigned char header[2] = {0x55, 0xAA};

// 发送左右轮速控制速度共用体
union SendData
{
    short d;
    unsigned char data[2];
};

union SendData angle[7];

// 接收数据（左轮速、右轮速、角度）共用体
union ReceiveData
{
    short d;
    unsigned char data[2];
} leftVelNow, rightVelNow, angleNow;

class SerialNode : public rclcpp::Node
{
public:
    SerialNode() : Node("serial_node")
    {
        serial_driver_ = std::make_unique<drivers::serial_driver::SerialDriver>(get_node_logging_interface());

        // 初始化串口
        Serial_Init();
    }

    void Serial_Init()
    {
        try
        {
            serial_driver_->init_port("/dev/ttyTHS0", 115200);
            RCLCPP_INFO(this->get_logger(), "Serial port opened successfully.");
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Unable to open serial port: %s", e.what());
        }
    }

    void writeSpeed(int *p, unsigned char ctrlFlag)
    {
        unsigned char buf[20] = {0};
        int i, length = 0;

        for (int i = 0; i < 7; i++)
        {
            angle[i].d = p[i];
        }

        // 设置消息头
        for (i = 0; i < 2; i++)
            buf[i] = header[i];

        // 设置机器人左右轮速度
        length = 11;
        buf[2] = length;

        for (i = 0; i < 2; i++)
        {
            buf[i + 3] = angle[0].data[i];
            buf[i + 5] = angle[1].data[i];
            buf[i + 7] = angle[2].data[i];
            buf[i + 9] = angle[3].data[i];
            buf[i + 11] = angle[4].data[i];
        }

        // 预留控制指令
        buf[3 + length - 1] = ctrlFlag;

        // 设置校验值、消息尾
        buf[3 + length] = getCrc8(buf, 3 + length);
        buf[3 + length + 1] = ender[0];
        buf[3 + length + 2] = ender[1];

        // 通过串口下发数据
        serial_driver_->port()->write(buf, 20);
        rclcpp::sleep_for(1s);
    }

    bool readSpeed(unsigned char &checkFlag, unsigned char &statusInfo, unsigned char &errorInfo,
                   int *speedValues, int *positionValues)
    {
        unsigned char buf[150] = {0};
        size_t Receive_N = serial_driver_->port()->available();

        if (Receive_N == 27)
        {
            int n = serial_driver_->port()->read(buf, Receive_N);

            if (buf[0] != header[0] || buf[1] != header[1])
            {
                RCLCPP_ERROR(this->get_logger(), "Header error.");
                return false;
            }

            int length = buf[2];

            unsigned char checkSum = getCrc8(buf, 3 + length);
            if (checkSum != buf[3 + length])
            {
                RCLCPP_ERROR(this->get_logger(), "CRC error.");
                return false;
            }

            checkFlag = buf[3];
            statusInfo = buf[4];
            errorInfo = buf[5];

            // 读取速度值
            for (int i = 0; i < 5; i++)
            {
                speedValues[i] = (int16_t)((buf[6 + i * 2] << 8) | buf[7 + i * 2]);
            }

            // 读取位置值
            for (int i = 0; i < 5; i++)
            {
                positionValues[i] = (int16_t)((buf[16 + i * 2] << 8) | buf[17 + i * 2]);
            }

            return true;
        }
        return false;
    }

    unsigned char getCrc8(unsigned char *ptr, unsigned short len)
    {
        unsigned char crc = 0;
        while (len--)
        {
            crc ^= *ptr++;
            for (int i = 0; i < 8; i++)
            {
                if (crc & 0x01)
                    crc = (crc >> 1) ^ 0x8C;
                else
                    crc >>= 1;
            }
        }
        return crc;
    }

private:
    std::unique_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SerialNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
